#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/twist.hpp>

class LidarBehavior : public rclcpp::Node
{

public:
    LidarBehavior() : Node("lidar_behavior_node")
    {
        lidar_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "/scan", 10, std::bind(&LidarBehavior::lidar_callback, this, std::placeholders::_1));
        vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
    }

private:
    void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
    int nCount = 0;
    geometry_msgs::msg::Twist vel_msgs;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
};

void LidarBehavior::lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
    // TODO: 避障逻辑

    int nNum = msg->ranges.size();
    int nMid = nNum / 2;
    float fMidDist = msg->ranges[nMid];

    RCLCPP_INFO(this->get_logger(), "Min distance: %f", fMidDist);

    if (nCount > 0 ){
        nCount --;
        return;
    }

    if(fMidDist < 1.5){
        vel_msgs.angular.z = 0.5;
        nCount = 100;
    }
    else{
        vel_msgs.linear.x  = 0.1;
        //vel_msgs.angular.z = 0.0;
    }
    vel_pub_->publish(vel_msgs);
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<LidarBehavior>());
    rclcpp::shutdown();
    return 0;
}